#include "total.h"

// volatile uint16_t AD_L, AD_M, AD_R, D_AD_VALUE, AD_L_L, AD_R_R;
// volatile uint16_t B1_Max,B2_Max,B3_Max,B4_Max,B5_Max;
// volatile uint8_t B1,B2,B3,B4,B5;
// volatile uint16_t AD_L_Average[5],AD_R_Average[5];
// volatile uint16_t sign = 250;

// volatile static int LeftMax=0;
// volatile static int RightMax=0;
// volatile static int Left_Thersh=0;
// volatile static int Right_Thersh=0;
// volatile static int Left_Span=0;
// volatile static int Right_Span=0;

volatile uint8_t H0, H1, H2, H3, H4, H5, H6;

int tracking_Digital(void)
{
    if( DL_GPIO_readPins(Digital_D0_PORT,Digital_D0_PIN)==Digital_D0_PIN) H0 = 1;
    if( DL_GPIO_readPins(Digital_D0_PORT,Digital_D0_PIN)!=Digital_D0_PIN) H0 = 0;

    if( DL_GPIO_readPins(Digital_D1_PORT,Digital_D1_PIN)==Digital_D1_PIN) H1 = 1;
    if( DL_GPIO_readPins(Digital_D1_PORT,Digital_D1_PIN)!=Digital_D1_PIN) H1 = 0;

    if( DL_GPIO_readPins(Digital_D2_PORT,Digital_D2_PIN)==Digital_D2_PIN) H2 = 1;
    if( DL_GPIO_readPins(Digital_D2_PORT,Digital_D2_PIN)!=Digital_D2_PIN) H2 = 0;

    if( DL_GPIO_readPins(Digital_D3_PORT,Digital_D3_PIN)==Digital_D3_PIN) H3 = 1;
    if( DL_GPIO_readPins(Digital_D3_PORT,Digital_D3_PIN)!=Digital_D3_PIN) H3 = 0;

    if( DL_GPIO_readPins(Digital_D4_PORT,Digital_D4_PIN)==Digital_D4_PIN) H4 = 1;
    if( DL_GPIO_readPins(Digital_D4_PORT,Digital_D4_PIN)!=Digital_D4_PIN) H4 = 0;

    if( DL_GPIO_readPins(Digital_D5_PORT,Digital_D5_PIN)==Digital_D5_PIN) H5 = 1;
    if( DL_GPIO_readPins(Digital_D5_PORT,Digital_D5_PIN)!=Digital_D5_PIN) H5 = 0;

    if( DL_GPIO_readPins(Digital_D6_PORT,Digital_D6_PIN)==Digital_D6_PIN) H6 = 1;
    if( DL_GPIO_readPins(Digital_D6_PORT,Digital_D6_PIN)!=Digital_D6_PIN) H6 = 0;

    int error_digital = (H6 - H0)*5 + (H5 - H1)*3 + (H4 - H2);
    return error_digital;
}


// /**
//   * @brief 差值定位算法初始化
//   * @param 无
//   * @retval 无
//   */
// void tracking_Init(void)
// {
//     OLED_Clear();
// 	OLED_ShowString(48,0,(uint8_t*)"Testing...",8);
	
// 	/*计算D_AD_VALUE的平均值*/
// 	uint8_t i;
// 	for(i=0;i<5;i++)
// 	{
//         sensor_AD_GetValue();
// 		AD_R_Average[i] = ADC_Val4;
// 		AD_L_Average[i] = ADC_Val2;
// 		delay_ms(100);
// 	}
	
// 	AD_L = (uint16_t)((AD_L_Average[0]+AD_L_Average[1]+AD_L_Average[2]+AD_L_Average[3]+AD_L_Average[4])/ 5.0f);
// 	AD_R = (uint16_t)((AD_R_Average[0]+AD_R_Average[1]+AD_R_Average[2]+AD_R_Average[3]+AD_R_Average[4])/ 5.0f);
// 	D_AD_VALUE = AD_R - AD_L;

//     DL_GPIO_clearPins(LED_PORT, LED_LED1_PIN);
// 	while(1)
// 	{
//         static int button_temp = 0;
//         button_temp = button_Proc();
//         if(button_temp == 3)
//         {
//             DL_GPIO_setPins(LED_PORT, LED_LED1_PIN);
//             printf("L_MAX=%d,R_MAX=%d,L_Thersh:%d,R_Thersh:%d,Left_Span:%d,Right_Span:%d\r\n",LeftMax,RightMax,Left_Thersh,Right_Thersh,Left_Span,Right_Span);
//             delay_ms(1000);
//             button_temp = 0;
//             break;
//         }
	
//         sensor_AD_GetValue();
// 		AD_L_L = ADC_Val1;
// 		AD_L = ADC_Val2;
// 		AD_M = ADC_Val3;
// 		AD_R = ADC_Val4;
// 		AD_R_R = ADC_Val5;
		
// 		if(AD_L_L>B1_Max)
// 		{
// 			B1_Max = AD_L_L;
// 		}
// 		if(AD_L>LeftMax)	
// 		{
// 			LeftMax=AD_L;
// 			B2_Max = LeftMax;
// 			Left_Thersh=AD_M;
// 			Left_Span=(2*LeftMax-AD_L)*2-(AD_L-AD_R+D_AD_VALUE);
// 		}
// 		if(AD_M > B3_Max)
// 		{
// 			B3_Max = AD_M;
// 		}
// 		if(AD_R>RightMax)
// 		{
// 			RightMax=AD_R;
// 			B4_Max = RightMax;
// 			Right_Thersh=AD_M;
// 			Right_Span=(AD_R-2*RightMax)*2-(AD_L-AD_R+D_AD_VALUE);	
// 		}		
// 		if(AD_R_R > B5_Max)
// 		{
// 			B5_Max = AD_R_R;
// 		}
// 	}
// }


// /**
//   * @brief 差值定位算法
//   * @param 无
//   * @retval 小车位置
//   */
// int tracking_Enable(void)
// {
// 	int Data_Out;

// 	static char PosFlag=0;
// 	int PosFlagValue=(int)((LeftMax+RightMax-Left_Thersh-Right_Thersh)/3.0f);

// 	sensor_AD_GetValue();
// 	AD_L = ADC_Val2;
// 	AD_M = ADC_Val3;
// 	AD_R = ADC_Val4;
//     // OLED_ShowNum(0, 3, AD_L, 6, 8);
//     // OLED_ShowNum(0, 4, AD_M, 6, 8);
//     // OLED_ShowNum(0, 5, AD_R, 6, 8);
	
// 	Data_Out=(AD_L-AD_R+D_AD_VALUE);
// 	if(Data_Out>PosFlagValue)
// 	{
// 		PosFlag=1;
// 	}
// 	else if(Data_Out<-PosFlagValue)
// 	{
// 		PosFlag=0;
// 	}
// 	if(AD_M<Left_Thersh)
// 	{	
// 		if(Data_Out>PosFlagValue)
// 		{
// 			Data_Out=(2*LeftMax-AD_L)*2-Left_Span;
// 		}
// 		else if((Data_Out<PosFlagValue)&&(PosFlag==1))
// 		{
// 			Data_Out=abs((2*LeftMax-AD_L)*2-Left_Span);
// 		}
		
// 	} 
	
// 	if(AD_M<Right_Thersh)
// 	{	
// 		if(Data_Out<-PosFlagValue)
// 		{
// 			Data_Out=(AD_R-2*RightMax)*2-Right_Span;
// 		}
// 		else if((Data_Out>-PosFlagValue)&&(PosFlag==0))
// 		{
// 			Data_Out=-abs((AD_R-2*RightMax)*2-Right_Span);
// 		}
// 	}
// 	printf("Out:%d\r\n",Data_Out);
//     printf("%d %d %d %d %d",ADC_Val1,ADC_Val2,ADC_Val3,ADC_Val4,ADC_Val5);
//     // OLED_ShowNum(0, 7, Data_Out, 6, 8);
// 	return Data_Out;
// }